# -*- coding: utf-8 -*-
'''
Created on 20.08.2019

@author: yu03
'''
import matplotlib.pyplot as plt
import numpy as np
import datetime
from PyUeye import file_name, file_path, txt_name, doc_name
import os
import matplotlib.pyplot as plt
import cv2
from scipy import signal
from FFT_Interpolation import FFT_interpolation_boxcar, FFT_interpolation_2, FFT_interpolation_compare, FFT_cal
from scipy.optimize import curve_fit
from scipy import signal


def running_mean(x, N):
    cumsum = np.cumsum(np.insert(x, 0, 0)) 
#     return np.concatenate((np.zeros(N-1), (cumsum[N:] - cumsum[:-N]) / float(N)))
    return (cumsum[N:] - cumsum[:-N]) / float(N)

'''
    读取数据 (4通道)
'''
def Read_Data_4Ch(name):
    '''
        Return Data in File (4 Channels: Data_Ch1, Data_Ch2, Data_Ch3, Data_Ch4)
        File name required (default path)
    '''
    print('Reading Data')
    with open(name,'r') as fid:
        line=''
        while line[0:4] != '----':
            line = fid.readline()
            print(line)
            if line[0:2] == 'Fs':
                p, q, m, n = line.strip().split(' ')
                Fs = float(m)
                print('Fs = %f\n'%Fs)
        out_str = fid.readlines()
    Data_Ch1, Data_Ch2, Data_Ch3, Data_Ch4 = [], [], [], []
    for line in out_str:
        a, b, c, d= line.strip().split(', ')
        Data_Ch1.append(float(a))
        Data_Ch2.append(float(b))
        Data_Ch3.append(float(c))
        Data_Ch4.append(float(d))    
    Data_Ch1 = np.array(Data_Ch1)
    Data_Ch2 = np.array(Data_Ch2)
    Data_Ch3 = np.array(Data_Ch3)
    Data_Ch4 = np.array(Data_Ch4)
    return Data_Ch1, Data_Ch2, Data_Ch3, Data_Ch4, Fs
    
def Read_Data_2Ch(name):
    '''
        Return Data in File (4 Channels: Data_Ch1, Data_Ch2)
        File name required (default path)
    '''
    print('Reading Data')
    with open(name,'r') as fid:
        line=''
        while line[0:4] != '----':
            line = fid.readline()
            print(line)
            if line[0:2] == 'Fs':
                p, q, m, n = line.strip().split(' ')
                fs_cam = float(m)
                print('fs_cam = %f\n'%fs_cam)
        out_str = fid.readlines()
    Data_Ch1, Data_Ch2 = [], []
    for line in out_str:
        a, b= line.strip().split(', ')
        Data_Ch1.append(float(a))
        Data_Ch2.append(float(b))
    Data_Ch1 = np.array(Data_Ch1)
    Data_Ch2 = np.array(Data_Ch2)
    return Data_Ch1, Data_Ch2, fs_cam

Lamda = 633e-9
pix_size = 5.3e-6
V_x, V_y, V_z = 0, 0, 0

 
'''
    读取数据
'''

now = datetime.datetime.now()
hor_f_fit_set, hor_phase_centers, fs_cam = Read_Data_2Ch(txt_name)
hor_angle = (V_x-Lamda*hor_f_fit_set/2)*1e6 ### urad
hor_length = np.unwrap(hor_phase_centers)/4/np.pi*Lamda*1e9 ### nm

hor_angle_mean = running_mean(hor_angle, 50)
hor_length_mean = running_mean(hor_length, 50)


hor_length = hor_length[20:]
print(len(hor_length))
hor_angle = hor_angle[20:]

if 1: ### Plotting Noise
    
    ###Noise_PSD####### linearity range
    start_cutting, stop_cutting = 0, len(hor_phase_centers)
#     start_cutting, stop_cutting = 20, 3000
    
    selected_range = np.linspace(start_cutting, stop_cutting, stop_cutting-start_cutting)
    
    ############### Plotting
     
    plt.figure(1)
    plt.gcf().set_size_inches(18,9)
    ax1 = plt.subplot(3,2,1)
    range = [0, len(hor_angle)]
    x = (np.arange(range[1]-range[0])+range[0])/fs_cam
    y = hor_angle
    plt.plot(x, y, color='blue', marker=' ', label='angle_hor')
    
#     a = y.reshape(len(y)//10,-1)
#     y_avr = np.mean(a,axis=1)
#     y = y_avr
#     plt.plot(x[::10], y_avr, color='red', marker=' ', label='10-point filt')
    plt.title("Horizontal Tilting")
    plt.ylabel("Horizontal Angle (urad)")
    plt.xlabel("Time (s)")
    plt.grid(which='both', axis='both')
    plt.legend()
        
    ax2 = plt.subplot(3,2,2)
    range = [0, len(hor_length)]
    x = (np.arange(range[1]-range[0])+range[0])/fs_cam
    y = hor_length
    plt.plot(x, y, color='blue', marker=' ', label='length_hor')
    
#     a = hor_length.reshape(len(hor_length)//10,-1)
#     hor_length_avr = np.mean(a,axis=1)
#     y = hor_length_avr
#     plt.plot(x[::10], hor_length_avr, color='red', marker=' ', label='10-point filt')
    plt.title("Horizontal Phase")
    plt.ylabel("Horizontal Length (nm)]")
    plt.xlabel("Time (s)")
    plt.grid(which='both', axis='both')
    plt.legend()  
    
    ax3 = plt.subplot(3,2,3)
    range = [0, len(hor_angle)]
    x = (np.arange(range[1]-range[0])+range[0])/fs_cam
    y = (hor_angle-np.average(hor_angle))[range[0]:range[1]]
    plt.plot(x, y, color='blue', marker=' ', label='angle_hor')
    
#     a = y.reshape(len(y)//10,-1)
#     y_avr = np.mean(a,axis=1)
#     y = y_avr
#     plt.plot(x[::10], y_avr, color='red', marker='o', markersize=3, label='10-point filt')
    plt.title("Horizontal Tilting")
    plt.ylabel("Horizontal Angle (urad)")
    plt.xlabel("Time (s)")
    plt.grid(which='both', axis='both')
    plt.legend()
    
    ax4 = plt.subplot(3,2,4)
    range = [0, len(hor_length)]
    x = (np.arange(range[1]-range[0])+range[0])/fs_cam
    y = (hor_length-np.average(hor_length))[range[0]:range[1]]
    plt.plot(x, y, color='blue', marker=' ', label='length_hor')
    
#     a = y.reshape(len(y)//10,-1)
#     y_avr = np.mean(a,axis=1)
#     y = y_avr
#     plt.plot(x[::10], y_avr, color='red', marker='o', markersize=3, label='10-point filt')
    plt.title("Horizontal Length")
    plt.ylabel("Horizontal Length (nm)")
    plt.xlabel("Time (s)")
    plt.grid(which='both', axis='both')
    plt.legend()
    
    ax5 = plt.subplot(3,2,5)
    f_line, PS = signal.welch(hor_angle, fs_cam, nperseg=len(hor_angle))
    PSD = np.sqrt(PS)
    plt.loglog(f_line, PSD, 'b')
    plt.title("Horizontal Tilting Noise")
    plt.ylabel("urad/$\sqrt{Hz}$)")
    plt.xlabel("Freq. (Hz)")
    plt.grid(which='both', axis='both')
    plt.ylim(1e-5, 1e0) # for Ramp/20s noise
#     plt.ylim(1e-4, 0.1) # for noise
    
    ax6 = plt.subplot(3,2,6)
    f_line, PS = signal.welch(hor_length, fs_cam, nperseg=len(hor_length))
    PSD = np.sqrt(PS)
    plt.loglog(f_line, PSD, 'b')
    plt.title("Horizontal Length Noise")
    plt.ylabel("urad/$\sqrt{Hz}$)")
    plt.xlabel("Freq. (Hz)")
    plt.grid(which='both', axis='both')
    plt.ylim(1e-5, 1e0) # for Ramp/20s noise
#     plt.ylim(1e-6, 2e-2) # for square wave
#     plt.ylim(1e-6, 1e-3)# for noise
    
    plt.tight_layout()
    plt.show()



if 0: ### Plotting Nonlinearity
    
    ############### linearity range
    start_cutting, stop_cutting = 0, len(hor_phase_centers)
    start_cutting, stop_cutting = start_cutting/fs_cam, stop_cutting/fs_cam
#     start_cutting, stop_cutting = 1.6, 6.4
    zoom_time = start_cutting, stop_cutting
    
#     start_cutting, stop_cutting = 1, 1.5 ### in second   
#     zoom_time = [1.2, 1.4]
    
    start_cutting, stop_cutting = int(start_cutting*fs_cam), int(stop_cutting*fs_cam)
    selected_range = np.linspace(start_cutting, stop_cutting, stop_cutting-start_cutting)
    selected_time = (np.arange(stop_cutting-start_cutting)+start_cutting)/fs_cam    
    zoom_range = [zoom_time[0]*fs_cam, zoom_time[1]*fs_cam]
    
    
    ############### Plotting
     
    plt.figure(1)
    plt.gcf().set_size_inches(18,9)
    ax1 = plt.subplot(3,2,1)
    range = [0, len(hor_angle)]
    x = (np.arange(range[1]-range[0])+range[0])/fs_cam
    y = hor_angle
    plt.plot(x, y, color='blue', marker=' ')
    measured_angle = hor_angle[start_cutting:stop_cutting]
    [p, q] = np.polyfit(np.linspace(0, len(measured_angle)-1, len(measured_angle)), measured_angle, 1)
    linear_fit = np.linspace(0, len(measured_angle)-1, len(measured_angle))*p + q
    hor_angle_nonlinearity = (measured_angle - linear_fit)
    plt.plot(selected_time, linear_fit, color='red')
    plt.title("Horizontal Tilting")
    plt.ylabel("Horizontal Angle (urad)")
    plt.xlabel("Time (s)")
    plt.grid(which='both', axis='both')
        
    ax2 = plt.subplot(3,2,2)
    range = [0, len(hor_length)]
    x = (np.arange(range[1]-range[0])+range[0])/fs_cam
    y = hor_length
    plt.plot(x, y, color='blue', marker=' ')
    
    measured_length = hor_length[start_cutting:stop_cutting]
    [p, q] = np.polyfit(np.linspace(0, len(measured_length)-1, len(measured_length)), measured_length, 1)
    linear_fit = np.linspace(0, len(measured_length)-1, len(measured_length))*p + q
    hor_length_nonlinearity = (measured_length - linear_fit)
    plt.plot(selected_time, linear_fit, color='red')
    plt.title("Horizontal Length")
    plt.ylabel("Horizontal Length (nm)")
    plt.xlabel("Time (s)")
    plt.grid(which='both', axis='both')
      
    ax3 = plt.subplot(3,2,3)
    range = [start_cutting, stop_cutting]
    x = (np.arange(range[1]-range[0])+range[0])/fs_cam
    y = hor_angle_nonlinearity
    plt.plot(x, y, color='blue', marker=' ')
#     plt.plot(x[19:], running_mean(y, 20), 'r')
    plt.title("Horizontal Tilting")
    plt.ylabel("Horizontal Angle (urad)")
    plt.xlabel("Time (s)")
    plt.grid(which='both', axis='both')
       
    ax4 = plt.subplot(3,2,4)
    range = [start_cutting, stop_cutting]
    x = (np.arange(range[1]-range[0])+range[0])/fs_cam
    y = hor_length_nonlinearity
    plt.plot(x, y, color='blue', marker=' ')
#     plt.plot(x[19:], running_mean(y, 20), 'r')
    plt.title("Horizontal Length")
    plt.ylabel("Horizontal Length (nm)")
    plt.xlabel("Time (s)")
    plt.grid(which='both', axis='both')
      
    ax5 = plt.subplot(3,2,5)
    f_line, PS = signal.welch(hor_angle_nonlinearity, fs_cam, nperseg=len(hor_angle_nonlinearity))
    PSD = np.sqrt(PS)
    plt.loglog(f_line, PSD, 'b')
    
#     f_line_mean, PS_mean = signal.welch(running_mean(hor_angle_nonlinearity, 20), fs_cam, nperseg=len(running_mean(hor_angle_nonlinearity, 20)))
#     PSD_mean = np.sqrt(PS_mean)
#     plt.loglog(f_line_mean, PSD_mean, 'r')
    
    plt.title("Horizontal Tilting Noise")
    plt.ylaNoise_PSDPSD (urad/$\sqrt{Hz}$)")
    plt.xlabel("Freq. (Hz)")
    plt.grid(which='both', axis='both')
    plt.ylim(1e-4, 10) # for Ramp/20s noise
#     plt.ylim(1e-4, 0.1) # for noise
    
    ax6 = plt.subplot(3,2,6)
#     measured_length = (hor_length+ver_length)/2
#     [p, q] = np.polyfit(np.linspace(0, len(measured_length)-1, len(measured_length)), measured_length, 1)
#     linear_fit = np.linspace(0, len(measured_length)-1, len(measured_length))*p + q
#     length_nonlinearity = (measured_length - linear_fit)
#     plt.plot(x, length_nonlinearity, color='blue', marker=' ')
    f_line, PS = signal.welch(hor_length_nonlinearity, fs_cam, nperseg=len(hor_length_nonlinearity))
    PSD = np.sqrt(PS)
    plt.loglog(f_line, PSD, 'b')
    plt.title("Horizontal Length Noise")
    plt.ylaNoise_PSDPSD (nm/$\sqrt{Hz}$)")
    plt.xlabel("Freq. (Hz)")
    plt.grid(which='both', axis='both')
    plt.ylim(1e-4, 1e1) # for Ramp/20s noise
    
    plt.tight_layout()
    plt.show()



    
    
